#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from turtlesim.srv import TeleportRelative
from turtlesim.srv import SetPen
from std_srvs.srv import Empty
import math
import sys

def draw_colorSquareSpiral():
    # ROS节点初始化
    rospy.init_node('turtle_colorSquareSpiral')

    # 发现/turtle1/teleport_relative服务后，创建一个服务客户端，连接名为/turtle1/teleport_relative的service
    rospy.wait_for_service( '/turtle1/teleport_relative')
    try:
        move_turtle = rospy.ServiceProxy( '/turtle1/teleport_relative', TeleportRelative)
        set_pen = rospy.ServiceProxy('/turtle1/set_pen', SetPen)
        reset = rospy.ServiceProxy('/reset', Empty)

        # 恢复默认值
        reset()

        # 画彩色螺旋线
        step = 0.04
        penWidth = 3
        for x in range(200):
            i = x%4
            if i==0:
                set_pen(255,0,0,penWidth,0)         # Args: r g b width off
            elif i==1:
                 set_pen(0,255,0,penWidth,0) 
            elif i==2:
                 set_pen(255,0,255,penWidth,0)
            elif i==3:
                 set_pen(255,255,0,penWidth,0)  

            response = move_turtle(x*step, 91/180*math.pi)

        return response
    except (rospy.ServiceException, rospy.ROSException, e):
        print("Service call failed: %s" % e)
        return False

if __name__ == "__main__":
    draw_colorSquareSpiral()
    print("Turtle draws a color square spiral successfully!")


# rosservice info /turtle1/teleport_relative 
# Node: /turtlesim
# URI: rosrpc://dsc-Alienware-m17-R3:50919
# Type: turtlesim/TeleportRelative
# Args: linear angular

# rosservice info /turtle1/set_pen 
# Node: /turtlesim
# URI: rosrpc://dsc-Alienware-m17-R3:50919
# Type: turtlesim/SetPen
# Args: r g b width off

# rosservice info /reset
# Node: /turtlesim
# URI: rosrpc://dsc-Alienware-m17-R3:50919
# Type: std_srvs/Empty
# Args: 


# ===== colorSquareSpiral.py ======
# import turtle

# turtle.bgcolor("black")
# t = turtle.Pen()
# colors = ["red", "yellow", "blue", "green"]

# for x in range(200):
#     t.pencolor(colors[x%4])
#     t.forward(x)
#     t.left(91)